// Copyright (C) 2005, 2006 International Business Machines and others.
// All Rights Reserved.
// This code is published under the Eclipse Public License.
//
// Authors:  Andreas Waechter              IBM    2005-10-127

#include "LuksanVlcek1.hpp"

#include <cmath>
#include <cstdio>

using namespace Ipopt;

LuksanVlcek1::LuksanVlcek1(
   Number g_l,
   Number g_u
)
   : g_l_(g_l),
     g_u_(g_u)
{ }

bool LuksanVlcek1::InitializeProblem(
   Index N
)
{
   N_ = N;
   if( N_ <= 2 )
   {
      printf("N needs to be at least 3.\n");
      return false;
   }
   return true;
}

// returns the size of the problem
bool LuksanVlcek1::get_nlp_info(
   Index&          n,
   Index&          m,
   Index&          nnz_jac_g,
   Index&          nnz_h_lag,
   IndexStyleEnum& index_style
)
{
   // The problem described in LuksanVlcek1.hpp has 4 variables, x[0] through x[3]
   n = N_;

   m = N_ - 2;

   nnz_jac_g = m * 3;

   nnz_h_lag = n + n - 1;

   // use the C style numbering of matrix indices (starting at 0)
   index_style = TNLP::C_STYLE;

   return true;
}

// returns the variable bounds
bool LuksanVlcek1::get_bounds_info(
   Index   n,
   Number* x_l,
   Number* x_u,
   Index   m,
   Number* g_l,
   Number* g_u
)
{
   // none of the variables have bounds
   for( Index i = 0; i < n; i++ )
   {
      x_l[i] = -1e20;
      x_u[i] = 1e20;
   }

   // Set the bounds for the constraints
   for( Index i = 0; i < m; i++ )
   {
      g_l[i] = g_l_;
      g_u[i] = g_u_;
   }

   return true;
}

// returns the initial point for the problem
bool LuksanVlcek1::get_starting_point(
   Index   n,
   bool    init_x,
   Number* x,
   bool    init_z,
   Number* /*z_L*/,
   Number* /*z_U*/,
   Index   /*m*/,
   bool    init_lambda,
   Number* /*lambda*/
)
{
   if( !init_x || init_z || init_lambda )
   {
      return false;
   }

   // set the starting point
   for( Index i = 0; i < n / 2; i++ )
   {
      x[2 * i] = -1.2;
      x[2 * i + 1] = 1.;
   }
   if( n != 2 * (n / 2) )
   {
      x[n - 1] = -1.2;
   }

   return true;
}

// returns the value of the objective function
bool LuksanVlcek1::eval_f(
   Index         /*n*/,
   const Number* x,
   bool          /*new_x*/,
   Number&       obj_value
)
{
   obj_value = 0.;
   for( Index i = 0; i < N_ - 1; i++ )
   {
      Number a1 = x[i] * x[i] - x[i + 1];
      Number a2 = x[i] - 1.;
      obj_value += 100. * a1 * a1 + a2 * a2;
   }

   return true;
}

// return the gradient of the objective function grad_{x} f(x)
bool LuksanVlcek1::eval_grad_f(
   Index         /*n*/,
   const Number* x,
   bool          /*new_x*/,
   Number*       grad_f
)
{
   grad_f[0] = 0.;
   for( Index i = 0; i < N_ - 1; i++ )
   {
      grad_f[i] += 400. * x[i] * (x[i] * x[i] - x[i + 1]) + 2. * (x[i] - 1.);
      grad_f[i + 1] = -200. * (x[i] * x[i] - x[i + 1]);
   }

   return true;
}

// return the value of the constraints: g(x)
bool LuksanVlcek1::eval_g(
   Index         /*n*/,
   const Number* x,
   bool          /*new_x*/,
   Index         /*m*/,
   Number*       g
)
{
   for( Index i = 0; i < N_ - 2; i++ )
   {
      g[i] = 3. * std::pow(x[i + 1], 3.) + 2. * x[i + 2] - 5. + std::sin(x[i + 1] - x[i + 2]) * std::sin(x[i + 1] + x[i + 2])
             + 4. * x[i + 1] - x[i] * std::exp(x[i] - x[i + 1]) - 3.;
   }

   return true;
}

// return the structure or values of the jacobian
bool LuksanVlcek1::eval_jac_g(
   Index         /*n*/,
   const Number* x,
   bool          /*new_x*/,
   Index         /*m*/,
   Index         /*nele_jac*/,
   Index*        iRow,
   Index*        jCol,
   Number*       values
)
{
   if( values == NULL )
   {
      // return the structure of the jacobian

      Index ijac = 0;
      for( Index i = 0; i < N_ - 2; i++ )
      {
         iRow[ijac] = i;
         jCol[ijac] = i;
         ijac++;
         iRow[ijac] = i;
         jCol[ijac] = i + 1;
         ijac++;
         iRow[ijac] = i;
         jCol[ijac] = i + 2;
         ijac++;
      }
   }
   else
   {
      // return the values of the jacobian of the constraints

      Index ijac = 0;
      for( Index i = 0; i < N_ - 2; i++ )
      {
         // x[i]
         values[ijac] = -(1. + x[i]) * std::exp(x[i] - x[i + 1]);
         ijac++;
         // x[i+1]
         values[ijac] = 9. * x[i + 1] * x[i + 1] + std::cos(x[i + 1] - x[i + 2]) * std::sin(x[i + 1] + x[i + 2])
                        + std::sin(x[i + 1] - x[i + 2]) * std::cos(x[i + 1] + x[i + 2]) + 4. + x[i] * std::exp(x[i] - x[i + 1]);
         ijac++;
         // x[i+2]
         values[ijac] = 2. - std::cos(x[i + 1] - x[i + 2]) * std::sin(x[i + 1] + x[i + 2])
                        + std::sin(x[i + 1] - x[i + 2]) * std::cos(x[i + 1] + x[i + 2]);
         ijac++;
      }
   }

   return true;
}

//return the structure or values of the hessian
bool LuksanVlcek1::eval_h(
   Index         /*n*/,
   const Number* x,
   bool          /*new_x*/,
   Number        obj_factor,
   Index         /*m*/,
   const Number* lambda,
   bool          /*new_lambda*/,
   Index         nele_hess,
   Index*        iRow,
   Index*        jCol,
   Number*       values
)
{
   if( values == NULL )
   {
      Index ihes = 0;
      for( Index i = 0; i < N_; i++ )
      {
         iRow[ihes] = i;
         jCol[ihes] = i;
         ihes++;
         if( i < N_ - 1 )
         {
            iRow[ihes] = i;
            jCol[ihes] = i + 1;
            ihes++;
         }
      }
      DBG_ASSERT(ihes == nele_hess);
      (void) nele_hess;
   }
   else
   {
      Index ihes = 0;
      for( Index i = 0; i < N_; i++ )
      {
         // x[i],x[i]
         if( i < N_ - 1 )
         {
            values[ihes] = obj_factor * (2. + 400. * (3. * x[i] * x[i] - x[i + 1]));
            if( i < N_ - 2 )
            {
               values[ihes] -= lambda[i] * (2. + x[i]) * std::exp(x[i] - x[i + 1]);
            }
         }
         else
         {
            values[ihes] = 0.;
         }
         if( i > 0 )
         {
            // x[i+1]x[i+1]
            values[ihes] += obj_factor * 200.;
            if( i < N_ - 1 )
            {
               values[ihes] += lambda[i - 1]
                               * (18. * x[i] - 2. * std::sin(x[i] - x[i + 1]) * std::sin(x[i] + x[i + 1])
                                  + 2. * std::cos(x[i] - x[i + 1]) * std::cos(x[i] + x[i + 1]) - x[i - 1] * std::exp(x[i - 1] - x[i]));
            }
         }
         if( i > 1 )
         {
            // x[i+2]x[i+2]
            values[ihes] += lambda[i - 2]
                            * (-2. * std::sin(x[i - 1] - x[i]) * std::sin(x[i - 1] + x[i]) - 2. * std::cos(x[i - 1] - x[i]) * std::cos(x[i - 1] + x[i]));
         }
         ihes++;

         if( i < N_ - 1 )
         {
            // x[i],x[i+1]
            values[ihes] = obj_factor * (-400. * x[i]);
            if( i < N_ - 2 )
            {
               values[ihes] += lambda[i] * (1. + x[i]) * std::exp(x[i] - x[i + 1]);
            }
            /*
             if (i>0) {
             // x[i+1],x[i+2]
             values[ihes] +=
             lambda[i-1]*(  std::sin(x[i]-x[i+1])*std::sin(x[i]+x[i+1])
             + std::cos(x[i]-x[i+1])*std::cos(x[i]+x[i+1])
             - std::cos(x[i]-x[i+1])*std::cos(x[i]+x[i+1])
             - std::sin(x[i]-x[i+1])*std::sin(x[i]+x[i+1])
             );
             }
             */
            ihes++;
         }
      }
      DBG_ASSERT(ihes == nele_hess);
   }

   return true;
}

void LuksanVlcek1::finalize_solution(
   SolverReturn               /*status*/,
   Index                      /*n*/,
   const Number*              /*x*/,
   const Number*              /*z_L*/,
   const Number*              /*z_U*/,
   Index                      /*m*/,
   const Number*              /*g*/,
   const Number*              /*lambda*/,
   Number                     /*obj_value*/,
   const IpoptData*           /*ip_data*/,
   IpoptCalculatedQuantities* /*ip_cq*/
)
{ }
